#ifndef PLAN_H
#define PLAN_H

#include "printf.h"
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include "log.h"

typedef struct
{
    float time_lim;
    float poly_const[6];
    float pos_cur;
    float pos_start;
    float pos_end;
} JointPlanner_TypeDef;

#define PLAN_ERR(x) (-2000 - x)          // -2001~-3000
#define E_PLAN_NO_ERROR 0                // 没有错误
#define E_PLAN_PARA_INVALID PLAN_ERR(1)  // 未知规划类型
#define E_PLAN_POS_TOO_CLOSE PLAN_ERR(2) // 起止点相距太近

#define PLAN_EPS 1e-3


JointPlanner_TypeDef *joint_planner_new(void);
int joint_planner_init(float time_lim, float vel_lim, float pos_start, float pos_end, JointPlanner_TypeDef *planner);
float joint_plan_move(JointPlanner_TypeDef *planner, float t);

#endif // PLAN_H
